﻿#include "cv.h"
#include "cvaux.h"
#include "highgui.h"
#include <cstring>
#include <vector>
#include <math.h>
#include <iostream>

using std::vector;
using std::cout;
using std::endl;

//static IplImage* image;
static vector<CvPoint>mypoints;
static double fps;
static int RunBlobTrackingAuto( CvCapture* pCap, CvBlobTrackerAuto* pTracker,char* fgavi_name = NULL, char* btavi_name = NULL ) {
    int                     OneFrameProcess = 0;
    int                     key;
    int                     FrameNum = 0;
    CvVideoWriter*          pFGAvi   = NULL;
    CvVideoWriter*          pBTAvi   = NULL;
    IplConvKernel*          elementE1  = cvCreateStructuringElementEx(1,3,0,1,CV_SHAPE_RECT);
    IplConvKernel*          elementE2  = cvCreateStructuringElementEx(3,1,1,0,CV_SHAPE_RECT);
    IplConvKernel*          elementD   = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_ELLIPSE);

    /* Main loop: */
    for( FrameNum=0; pCap && (key=cvWaitKey(OneFrameProcess?0:1))!=27; FrameNum++) {
        /* Main loop: */
        IplImage*   pImg0  = NULL;
        IplImage*   pImg   = NULL;
        IplImage*   pMask  = NULL;

        if(key!=-1) {
            OneFrameProcess = 1;
            if(key=='r')OneFrameProcess = 0;
        }

        pImg = cvQueryFrame(pCap);
        if(pImg == NULL) break;

        pImg0 = cvCloneImage(pImg);
        cvErode(pImg0,pImg0,elementE1,4);
        cvErode(pImg0,pImg0,elementE2,4);
        cvDilate(pImg0,pImg0,elementD,7);

        /* Process: */
        pTracker->Process(pImg0, pMask);

        if(fgavi_name)
            if(pTracker->GetFGMask()) {
                /* Debug FG: */
                IplImage*           pFG = pTracker->GetFGMask();
                CvSize              S = cvSize(pFG->width,pFG->height);
                static IplImage*    pI = NULL;

                if(pI==NULL)pI = cvCreateImage(S,pFG->depth,3);
                cvCvtColor( pFG, pI, CV_GRAY2BGR );

                if(fgavi_name) {
                    /* Save fg to avi file: */
                    if(pFGAvi==NULL) {
                        pFGAvi=cvCreateVideoWriter(
                                   fgavi_name,
                                   CV_FOURCC('x','v','i','d'),
                                   25,
                                   S );
                    }
                    cvWriteFrame( pFGAvi, pI );
                }

                if(pTracker->GetBlobNum()>0) {
                    /* Draw detected blobs: */
                    int i;
                    for(i=pTracker->GetBlobNum(); i>0; i--) {
                        CvBlob* pB = pTracker->GetBlob(i-1);
                        CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
                        CvSize  s = cvSize(MAX(1,cvRound(CV_BLOB_RX(pB))), MAX(1,cvRound(CV_BLOB_RY(pB))));
                        int c = cvRound(255*pTracker->GetState(CV_BLOB_ID(pB)));
                        cvEllipse( pI,
                                   p,
                                   s,
                                   0, 0, 360,
                                   CV_RGB(c,255-c,0), cvRound(1+(3*c)/255) );
				mypoints.push_back(p);
				vector<CvPoint>::iterator it=mypoints.begin();
				vector<CvPoint>::iterator it1;
				for(;it!=mypoints.end();it++)
				{
					CvScalar p;
					double speed;
					p.val[0]=255;
					it1=it+1;
					if(it1!=mypoints.end())
					{
						double m=(it->x-it1->x)*(it->x-it1->x)+(it->y-it1->y)*(it->y-it1->y);
						speed=sqrt(m)*fps;
						//cout<<"speed:"<<speed<<"px/s\n";
						//cvCircle( pI,*it, 2, p, 1, 8, 0 );
						cvLine( pI, *it, *it1, p,3,8 , 0 );
					}
				}

                    }   /* Next blob: */;
                }

                cvNamedWindow( "FG",0);
                cvShowImage( "FG",pI);
            }   /* Debug FG. */


        /* Draw debug info: */
        if(pImg) {
            /* Draw all information about test sequence: */
            char        str[1024];
            int         line_type = CV_AA;   // Change it to 8 to see non-antialiased graphics.
            CvFont      font;
            int         i;
            IplImage*   pI = cvCloneImage(pImg);

            cvInitFont( &font, CV_FONT_HERSHEY_PLAIN, 0.7, 0.7, 0, 1, line_type );

            for(i=pTracker->GetBlobNum(); i>0; i--) {
                CvSize  TextSize;
                CvBlob* pB = pTracker->GetBlob(i-1);
                CvPoint p = cvPoint(cvRound(pB->x*256),cvRound(pB->y*256));
                CvSize  s = cvSize(MAX(1,cvRound(CV_BLOB_RX(pB)*256)), MAX(1,cvRound(CV_BLOB_RY(pB)*256)));
                int c = cvRound(255*pTracker->GetState(CV_BLOB_ID(pB)));
				//change here

                cvEllipse( pI,
                           p,
                           s,
                           0, 0, 360,
                           CV_RGB(c,255-c,0), cvRound(1+(3*0)/255), CV_AA, 8 );
				//change here
				vector<CvPoint>::iterator it=mypoints.begin();
				vector<CvPoint>::iterator it1;
				for(;it!=mypoints.end();it++)
				{
					CvScalar p;
					p.val[0]=255;
					it1=it+1;
					if(it1!=mypoints.end())
					{
						//cvCircle( pI,*it, 2, p, 1, 8, 0 );
						cvLine( pI, *it, *it1, p,3,8, 0 );
					}
				}
                p.x >>= 8;
                p.y >>= 8;
                s.width >>= 8;
                s.height >>= 8;
                sprintf(str,"%03d",CV_BLOB_ID(pB));
                cvGetTextSize( str, &font, &TextSize, NULL );
                p.y -= s.height;
                cvPutText( pI, str, p, &font, CV_RGB(0,255,255));
                {
                    const char* pS = pTracker->GetStateDesc(CV_BLOB_ID(pB));

                    if(pS) {
                        char* pStr = strdup(pS);
                        char* pStrFree = pStr;

                        while (pStr && strlen(pStr) > 0) {
                            char* str_next = strchr(pStr,'\n');

                            if(str_next) {
                                str_next[0] = 0;
                                str_next++;
                            }

                            p.y += TextSize.height+1;
                            cvPutText( pI, pStr, p, &font, CV_RGB(0,255,255));
                            pStr = str_next;
                        }
                        free(pStrFree);
                    }
                }

            }   /* Next blob. */;

            cvNamedWindow( "Tracking", 0);
            cvShowImage( "Tracking",pI );

            if(btavi_name && pI) {
                /* Save to avi file: */
                CvSize      S = cvSize(pI->width,pI->height);
                if(pBTAvi==NULL) {
                    pBTAvi=cvCreateVideoWriter(
                               btavi_name,
                               CV_FOURCC('x','v','i','d'),
                               25,
                               S );
                }
                cvWriteFrame( pBTAvi, pI );
            }

            cvReleaseImage(&pI);
        }   /* Draw all information about test sequence. */
    }   /*  Main loop. */

    if(pFGAvi)cvReleaseVideoWriter( &pFGAvi );
    if(pBTAvi)cvReleaseVideoWriter( &pBTAvi );
    if(elementE1)cvReleaseStructuringElement( &elementE1 );
    if(elementE2)cvReleaseStructuringElement( &elementE2 );
    if(elementD)cvReleaseStructuringElement( &elementD );
    return 0;
}   /* RunBlobTrackingAuto */

int main(int argc, char* argv[]) {
    /* Main function: */
    CvCapture*                  pCap = NULL;
    CvBlobTrackerAutoParam1     param = {0};
    CvBlobTrackerAuto*          pTracker = NULL;

    /*output files*/
    char *const track_file = "res0.txt";
    char *const fgavi_name = "res1.avi";
    char *const btavi_name = "res2.avi";

    /* Create source video: */
    if(argc==2 && argv[1])
        pCap = cvCaptureFromFile(argv[1]);

    if(pCap==NULL) {
        printf("Can't open %s file\n",argv[1]);
        return -1;
    }
    fps=cvGetCaptureProperty(pCap,CV_CAP_PROP_FPS);
	/*int frameH    = (int) cvGetCaptureProperty(pCap, CV_CAP_PROP_FRAME_HEIGHT);
    int frameW    = (int) cvGetCaptureProperty(pCap, CV_CAP_PROP_FRAME_WIDTH);
	CvSize sz;//图像尺寸
    sz.height =frameH;
    sz.width = frameW;
    image = cvCreateImage(sz,IPL_DEPTH_8U ,1);
	for(int i=0;i<sz.height;i++)
     {
         for(int j=0;j<sz.width ;j++)
        {
           CvScalar p = cvGet2D(image,i,j);//(j,i)
		   p.val[0]=0;
		   cvSet2D(image,i,j,p);

         }

     }   */ //*/

    {
        /* Create autotracker module and its components: */
        param.FGTrainFrames = 0;

        /* Create FG Detection module: */
        param.pFG = cvCreateFGDetectorBase(CV_BG_MODEL_FGD,NULL);
        if(!param.pFG)
            puts("Can not create FGDetector module");
        param.pFG->SetNickName("FG_0");

        /* Create Blob Entrance Detection module: */
        param.pBD = cvCreateBlobDetectorCC();
        if(!param.pBD)
            puts("Can not create BlobDetector module");
        param.pBD->SetNickName("BD_CC");

        /* Create blob tracker module: */
        param.pBT = cvCreateBlobTrackerCCMSPF();
        if(!param.pBT)
            puts("Can not create BlobTracker module");
        param.pBT->SetNickName("CCMSPF");

        /* Create blob trajectory generation module: */
        param.pBTGen = cvCreateModuleBlobTrackGenYML();
        if(!param.pBTGen)
            puts("Can not create blob trajectory generation module");
        param.pBTGen->SetNickName("YML");
        param.pBTGen->SetFileName(track_file);

        /* Create blob trajectory post processing module: */
        param.pBTPP = cvCreateModuleBlobTrackPostProcKalman();
        if(!param.pBTPP)
            puts("Can not create blob trajectory post processing module");
        param.pBTPP->SetNickName("Kalman");

        param.UsePPData = 1;    // ÕâÐÐ¸ÉÂïµÄ£¿

        /* Create blob trajectory analysis module: */
        param.pBTA = cvCreateModuleBlobTrackAnalysisHistPVS();
        if(!param.pBTA)
            puts("Can not create blob trajectory analysis module");
        param.pBTA->SetNickName("HistPVS");

        /* Create whole pipline: */
        pTracker = cvCreateBlobTrackerAuto1(&param);
        if(!pTracker)
            puts("Can not create BlobTrackerAuto");
    }

    /* Run pipeline: */
    RunBlobTrackingAuto( pCap, pTracker, fgavi_name, btavi_name );

    /* Release modules: */
    {
        if(param.pBT)cvReleaseBlobTracker(&param.pBT);
        if(param.pBD)cvReleaseBlobDetector(&param.pBD);
        if(param.pBTGen)cvReleaseBlobTrackGen(&param.pBTGen);
        if(param.pBTA)cvReleaseBlobTrackAnalysis(&param.pBTA);
        if(param.pFG)cvReleaseFGDetector(&param.pFG);
        if(pTracker)cvReleaseBlobTrackerAuto(&pTracker);
    }

    if(pCap)
        cvReleaseCapture(&pCap);
	//cvSaveImage("image1.jpg",image);
    return 0;
}
